import time
import openai
import re
import argparse
from airsim_wrapper import *
import math
import numpy as np
import os
import json
import time

client = AirSimWrapper1Uav()
client.takeoff()
# client.set_yaw(90) #设置飞行器的偏航角

current_pos = client.get_drone_position()  # [0, -1, -2]
print("cp = ", current_pos)
goal_pos = [50, 0, -5]
yaw = client.get_yaw()
print("yaw = ", yaw)
while np.linalg.norm(np.array(current_pos) - np.array(goal_pos)) > 5:  #0.5:
    # Check if there is at least 10 meters of distance in front of the drone
    distance = client.get_distance()
    if distance != 0:
        while distance < 13:
            print("yaw = ",  client.get_yaw())
            # Set the yaw angle to rotate the drone until there is at least 10 meters of clearance in the front
            client.set_yaw(yaw+30) # rotate 30 degrees at a time
            time.sleep(1) # wait for a second to measure the new distance
            print("yaw = ",  client.get_yaw())
            distance =  client.get_distance()
            print("distance = ",  distance)
            yaw = yaw + 30

    # Take a step towards the goal while maintaining the yaw angle
    step_size = 5 # take steps of 5 meters
    current_yaw = client.get_yaw()
    step_vector = [step_size * math.cos(current_yaw), step_size * math.sin(current_yaw), 0]
    new_pos = [current_pos[i]+step_vector[i] for i in range(3)]
    client.fly_to(new_pos)
    print("xxxxx =",  client.get_distance())
    
    # Face the goal again by calculating the new yaw angle
    current_pos = client.get_drone_position()
    yaw = math.atan2(goal_pos[1]-current_pos[1], goal_pos[0]-current_pos[0]) * 180 / math.pi
    client.set_yaw(yaw)
client.fly_to(goal_pos)

